﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace MassSpring
{
    public class Spring
    {
        public float SpringConstant;

        public float SpringLength;

        public float SpringDamping; //Spring damping constant

        public PhysicsNode FirstNode, SecondNode;

        public Spring(float k, float ce, float springLength)
        {
            this.SpringDamping = ce;
            this.SpringConstant = k;
            this.SpringLength = springLength;
        }

        private void applySpring(ref PhysicsNode first, ref PhysicsNode second)
        {
            // Apply force to node1
            // The force is equal to the spring constant k * -x, the extension of the spring.

            // Calculate the distance between the nodes
            Vector3 dir;
            dir.X = first.Position.X - second.Position.X;
            dir.Y = first.Position.Y - second.Position.Y;
            dir.Z = first.Position.Z - second.Position.Z;

            float dist = dir.Length(); //Save distance for later: sqrt is expensive

            // Calculate the magnitude of the force:
            float forceMag = SpringConstant * -(dist - SpringLength);

            // Apply in the proper direction - normalize the diff in position
            dir.X = dir.X / dist;
            dir.Y = dir.Y / dist;
            dir.Z = dir.Z / dist;

            //Apply force
            first.AccumForce.X = first.AccumForce.X + dir.X * forceMag;
            first.AccumForce.Y = first.AccumForce.Y + dir.Y * forceMag;
            first.AccumForce.Z = first.AccumForce.Z + dir.Z * forceMag;
            //Do the same for the second node. This is the same force but inverted.

            second.AccumForce.X = second.AccumForce.X - dir.X * forceMag;
            second.AccumForce.Y = second.AccumForce.Y - dir.Y * forceMag;
            second.AccumForce.Z = second.AccumForce.Z - dir.Z * forceMag;
            //Vector3.Multiply(ref dir, -forceMag, out SecondNode.AccumForce);

            //Apply edge friction for both nodes -Ce*(Vi-Vj)
            first.AccumForce.X =
                first.AccumForce.X - SpringDamping * (first.Velocity.X - second.Velocity.X);
            first.AccumForce.Y =
                first.AccumForce.Y - SpringDamping * (first.Velocity.Y - second.Velocity.Y);
            first.AccumForce.Z =
                first.AccumForce.Z - SpringDamping * (first.Velocity.Z - second.Velocity.Z);

            second.AccumForce.X =
                second.AccumForce.X - SpringDamping * (second.Velocity.X - first.Velocity.X);
            second.AccumForce.Y =
                second.AccumForce.Y - SpringDamping * (second.Velocity.Y - first.Velocity.Y);
            second.AccumForce.Z =
                second.AccumForce.Z - SpringDamping * (second.Velocity.Z - first.Velocity.Z);
        }

        public void Apply() 
        {
            if (FirstNode != null &&
                SecondNode != null)
            {
                applySpring(ref FirstNode, ref SecondNode);
            }
        }

        public void Attach(PhysicsNode n1, PhysicsNode n2) 
        {
            this.FirstNode = n1;
            this.SecondNode = n2;
        }

    }
}
